#include <inc/types.h>
#include <inc/apic.h>
#include <inc/pmap.h>
#include <inc/x86.h>


uint32_t read_ioapic(uint16_t sel)
{
    uint32_t val;
    *((volatile uint32_t *) 0xfec00000) = sel;
    val = *((volatile uint32_t*)0xfec00010);
    return val;
}

void write_ioapic(uint16_t sel, uint32_t data)
{
    *((volatile uint32_t *) 0xfec00000) = sel;
    *((volatile uint32_t *) 0xfec00010) = data;
}

uint32_t read_lapic(uint32_t offset)
{
    return *(volatile uint32_t *)((uint64_t)0xfee00000 + offset);
}

void write_lapic(uint32_t offset, uint32_t data)
{
    *(volatile uint32_t *)((uint64_t)0xfee00000 + offset) = data;
    read_lapic(offset);
}

int ioapic_tmp_map(uint64_t addr)
{
    pml_tmp4[pml_index(0, addr)] = (uint64_t)(page_index(addr)*0x1000 + 0x7);
    pml1[pml_index(3, addr)] = (uint64_t) pml2 + 0x7;
    pml2[pml_index(2, addr)] = (uint64_t) pml3 + 0x7;
    pml3[pml_index(1, addr)] = (uint64_t) pml_tmp4 + 0x7;

    return 1;
}	

int lapic_tmp_map(uint64_t addr)
{
    pml_tmp3[pml_index(0, addr)] = (uint64_t)(page_index(addr)*0x1000 + 0x7);
    pml1[pml_index(3, addr)] = (uint64_t) pml2 + 0x7;
    pml2[pml_index(2, addr)] = (uint64_t) pml3 + 0x7;
    pml3[pml_index(1, addr)] = (uint64_t) pml_tmp3 + 0x7;

    return 1;
}

static int disable_mcr(void)
{
    outb(0x22, 0x70);
    outb(0x23, 0x1);
    return 1;
}

int ioapic_init(void)
{
    int i, maxintr;
    uint32_t id;
    disable_mcr();
    id = read_ioapic(0) >> 24;
    maxintr = (read_ioapic(0x01)>>16)&0xff;
    if(id != ioapicid)
    {
	cprintf("the two id isn't equal%d, %d\n", id, ioapicid);
	//write_ioapic(0x0, ioapicid);
    }

    for(i = 0; i <= maxintr; i++)
    {
	write_ioapic(0x10 + i*2, 0x10000 | (32 + i));
	write_ioapic(0x10 + i*2 +1, 0x1<<24);
    }

    cprintf("ioapic is enabled\n");
    return 1;
}

void enable_irq(uint32_t irq, uint32_t cpu)
{
    write_ioapic((0x10 + (irq-32)*2), irq);
    write_ioapic((0x10 + (irq-32)*2 + 1), cpu<<24);
}

int enable_lapic(void)
{
    uint32_t i = read_lapic(0xf0);
    if(i & 0x100)
	lock_cprintf("lapic is enabled\n");
    else
    {
	i |= 0x100;
	write_lapic(0xf0, i);
    }
    /*	
	if(lapicid() == 0)
	{
	write_lapic(0x350, 0x700);
	write_lapic(0x340, 0x400);
	}
	else
	{
	write_lapic(0x350, 0x700 | 0x10000);
	write_lapic(0x340, 0x400 | 0x10000);
	}
    */

    write_lapic(0xf0, 0x100 | (32 + 31));
    write_lapic(0x370, 32+19);
    //	write_lapic(0x350, 0x700);
    //	write_lapic(0x360, 0x400);

    write_lapic(0x280, 0);
    write_lapic(0x280, 0);

    write_lapic(0x310, 0);
    write_lapic(0x300, 0x80000|0x500|0x8000|0x0);
    while(read_lapic(0x300) & 0x1000);

    write_lapic(0xb0, 0);
    write_lapic(0x80, 0);

    return 1;
}

int apic_wait(void)
{
    uint64_t i = 10000000;
    while(read_lapic(0x300)&DELIVER_ST)
    {
	delay(10);
	i --;
	if(i == 0)
	{
	    lock_cprintf("apic so busy\n");
	    return -1;
	}
    }
    return 0;
}

int delay(uint64_t cycles)
{
    uint64_t now;
    now = read_tsc();
    while((read_tsc() - now) < cycles)
	pause();
    return 1;
}

int apic_ipi(uint32_t id, uint32_t irq)
{
    write_lapic(0x310, id<<24);
    write_lapic(0x300, irq | FIXED | EDGE_TRIG | DE_ASS);
    apic_wait();
    if(read_lapic(0x300) & DELIVER_ST)
	return -1;
    else
	return 0;
}

int apic_ipi_init(uint32_t id)
{
    write_lapic(0x310, id<<24);
    write_lapic(0x300, id | 0x500 | 0x8000 | 0x4000);//ASSERT);
    apic_wait();
    delay(1500000);
    write_lapic(0x300, id | 0x500 | 0x8000 | 0x0);
    apic_wait();
    if(read_lapic(0x300) & DELIVER_ST)
	lock_cprintf("deliver failed\n");
    return 0;
}

int boot_ap(uint32_t pa, uint32_t id)
{
    lock_cprintf("sig before ap boot is 0x%x\n", *AP_BOOT_SIG);
    outb(0x70, 0xf);
    outb(0x71, 0x0a);

    uint16_t *warm_setv = (uint16_t*)0x467;
    warm_setv[0] = 0;
    warm_setv[1] = pa >> 4;	
    apic_ipi_init(id);
    delay(900000);
    while(read_lapic(0x300) & DELIVER_ST);

    write_lapic(0x310, id<<24);
    write_lapic(0x300, 0x600 | (pa>>12));
    apic_wait();
    while(read_lapic(0x300) & DELIVER_ST);

    delay(300000);

    apic_wait();
    write_lapic(0x310, id<<24);
    write_lapic(0x300, 0x600 | (pa>>12));
    apic_wait();
    while(read_lapic(0x300) & DELIVER_ST);

    delay(300000);

    while(*AP_BOOT_SIG != 0xdcba)
	lock_cprintf("sig is 0x%x\n", *AP_BOOT_SIG);
    lock_cprintf("sig after ap boot is 0x%x\n", *AP_BOOT_SIG);

    return 1;
}

uint32_t lapicid(void)
{
    return (read_lapic(0x20)>>24);
}
